#!/usr/bin/python
# -*- coding: UTF-8 -*-
import sys;
sys.path.append("..")
sys.path.append("../../")
from lib.BwRobotLib import BwRobotLib
from lib.MiniBotControlLib import MiniBotControlLib
from RobotManipulator.ManipulatorRobot import ManipulatorRobot
#from RobotManipulator.ManipulatorActions import ManipulatorActions
import keyboard
import time


robotlib = BwRobotLib()
manipulator = ManipulatorRobot(robotlib)
#robotlib.mclib.SetIsDebug(True)

#连接机器人设备
robotlib.connectRobot("192.168.3.46",55000)
#robotlib.connectRobot("10.10.100.254",55000)

#加载配置（兼容VS项目启动方式及文件直接启动方式）
import os,inspect
fileName = inspect.getframeinfo(inspect.currentframe()).filename
folderPath     = os.path.dirname(os.path.abspath(fileName))
cfile=folderPath+"/config.txt"
robotlib.loadConfig(cfile)

#配置操作臂
manipulator.bindIds(robotlib.module_ids)

#-----------------------------------------------------------------------------
#操作臂
#-----------------------------------------------------------------------------


#读取当前坐标
manipulator.readAngles()

#控制空间坐标
def controlCoordinate(key):
    cmd = key.name
    #print(cmd)
    #X
    if(cmd == 'i'):
        val = manipulator.spatial_coordinate[0] + 1
        manipulator.spatial_coordinate[0] = val
    if(cmd == 'k'):
        val = manipulator.spatial_coordinate[0] - 1
        manipulator.spatial_coordinate[0] = val

    #Y
    if(cmd == 'o'):
        val = manipulator.spatial_coordinate[1] + 1
        manipulator.spatial_coordinate[1] = val
    if(cmd == 'l'):
        val = manipulator.spatial_coordinate[1] - 1
        manipulator.spatial_coordinate[1] = val

    #Z
    if(cmd == 'p'):
        val = manipulator.spatial_coordinate[2] + 1
        manipulator.spatial_coordinate[2] = val
    if(cmd == 'm'):
        val = manipulator.spatial_coordinate[2] - 1
        manipulator.spatial_coordinate[2] = val

    #Roll
    if(cmd == 'z'):
        val = manipulator.spatial_coordinate[3] + 1
        manipulator.spatial_coordinate[3] = val
    if(cmd == 'x'):
        val = manipulator.spatial_coordinate[3] - 1
        manipulator.spatial_coordinate[3] = val

    #Pitch
    if(cmd == 'c'):
        val = manipulator.spatial_coordinate[4] + 1
        manipulator.spatial_coordinate[4] = val
    if(cmd == 'v'):
        val = manipulator.spatial_coordinate[4] - 1
        manipulator.spatial_coordinate[4] = val

     #Yaw
    if(cmd == 'b'):
        val = manipulator.spatial_coordinate[5] + 1
        manipulator.spatial_coordinate[5] = val
    if(cmd == 'n'):
        val = manipulator.spatial_coordinate[5] - 1
        manipulator.spatial_coordinate[5] = val

    manipulator.pushSpatialCoordinate()
    pass

#控制关节角度
def controlAngles(key):
    cmd = key.name
    #print(cmd)
    #I
    if(cmd == 'q'):
        val = manipulator.position_angles[0] + 1
        manipulator.position_angles[0] = robotlib.fixValue(val,180,-180)
    if(cmd == 'a'):
        val = manipulator.position_angles[0] - 1
        manipulator.position_angles[0] = robotlib.fixValue(val,180,-180)

    #T
    if(cmd == 'w'):
        val = manipulator.position_angles[1] + 1
        manipulator.position_angles[1] = robotlib.fixValue(val,90,-90)
    if(cmd == 's'):
        val = manipulator.position_angles[1] - 1
        manipulator.position_angles[1] = robotlib.fixValue(val,90,-90)

    #T
    if(cmd == 'e'):
        val = manipulator.position_angles[2] + 1
        manipulator.position_angles[2] = robotlib.fixValue(val,90,-90)
    if(cmd == 'd'):
        val = manipulator.position_angles[2] - 1
        manipulator.position_angles[2] = robotlib.fixValue(val,90,-90)

    #I
    if(cmd == 'r'):
        val = manipulator.position_angles[3] + 1
        manipulator.position_angles[3] = robotlib.fixValue(val,180,-180)
    if(cmd == 'f'):
        val = manipulator.position_angles[3] - 1
        manipulator.position_angles[3] = robotlib.fixValue(val,180,-180)

    #T
    if(cmd == 't'):
        val = manipulator.position_angles[4] + 1
        manipulator.position_angles[4] = robotlib.fixValue(val,90,-90)
    if(cmd == 'g'):
        val = manipulator.position_angles[4] - 1
        manipulator.position_angles[4] = robotlib.fixValue(val,90,-90)

    max = 180
    min = -180
    if(manipulator.ids_size <= 6):
        max = 90
        min = 0
     #G/I
    if(cmd == 'y'):      
        val = manipulator.position_angles[5] + 1
        manipulator.position_angles[5] = robotlib.fixValue(val,max,min)
    if(cmd == 'h'):
        val = manipulator.position_angles[5] - 1
        manipulator.position_angles[5] = robotlib.fixValue(val,max,min)

    #G
    if(cmd == 'u'):
        if(manipulator.ids_size > 6):        
           val = manipulator.position_angles[6] + 1
           manipulator.position_angles[6] = robotlib.fixValue(val,90,0)
    if(cmd == 'j'):
        if(manipulator.ids_size > 6):   
           val = manipulator.position_angles[6] - 1
           manipulator.position_angles[6] = robotlib.fixValue(val,90,0)

    manipulator.pushJointsAngles()
    pass


#处理控制键
def handleControlKeys(key):
     cmd = key.name
     #退出监测
     if(cmd == '0'):
        #keyboard.unhook_all()
        return

     if(cmd == 'i' or cmd == 'k' or cmd == 'o' or cmd == 'l' or cmd == 'p' or cmd == 'm' or cmd == 'z' or cmd == 'x' or cmd == 'c' or cmd == 'v' or cmd == 'b' or cmd == 'n'):
        controlCoordinate(key) 

     if(cmd == 'q' or cmd == 'a' or cmd == 'w' or cmd == 's' or cmd == 'e' or cmd == 'd' or cmd == 'r' or cmd == 'f' or cmd == 't' or cmd == 'g' or cmd == 'y' or cmd == 'h' or cmd == 'u' or cmd == 'j'):
        controlAngles(key) 


#-----------------------------------------------------------------------------
#差异代码分隔线
#-----------------------------------------------------------------------------


#控制关节角度
def controlTips():
    print("Keyboard to control angle: Increase / Reduce")
    print("Joint 1 : Q/A;")
    print("Joint 2 : W/S;")
    print("Joint 3 : E/D;")
    print("Joint 4 : R/F;")
    print("Joint 5 : T/G;")
    print("Joint 6 : Y/H;")
    print("Joint 7 : U/J;")

    print("Coordinate X : I/K;")
    print("Coordinate Y : O/L;")
    print("Coordinate Z : P/M;")
    print("Coordinate Roll: Z/X;")
    print("Coordinate Pitch: C/V;")
    print("Coordinate Yaw: B/N")

    print("-----------------------")
    print("0 :Exit")
    keyboard.hook(handleControlKeys)
    #参考Python 键盘/鼠标监听及控制：https://www.jianshu.com/p/8e508c6a05ce
    keyboard.wait('0')
    
    pass

#控制空间坐标
def controlCoordinateTips():
    print("Keyboard to control coordinate: Increase / Reduce")
    print("Coordinate X : I/K;")
    print("Coordinate Y : O/L;")
    print("Coordinate Z : P/M;")
    print("Coordinate Roll: Z/X;")
    print("Coordinate Pitch: C/V;")
    print("Coordinate Yaw: B/N")
    print("-----------------------")
    print("0 :Exit")

    #初始化一个姿态
    manipulator.position_angles[2] = 70
    manipulator.position_angles[4] = 30
    manipulator.pushJointsAngles()

    keyboard.hook(controlCoordinate)
    #参考Python 键盘/鼠标监听及控制：https://www.jianshu.com/p/8e508c6a05ce
    keyboard.wait('0')
    pass

while(1):
    print("Cmds:")
    print("1 : Control")
    print("2 : Control Coordinate")
    print("-----------------------")
    print("0 :Exit")
   
    cmd = robotlib.inputNum(int,"请输入：")
    if(cmd == 1):
       controlTips()
    if(cmd == 2):
       controlCoordinateTips()
    if(cmd == 0):
       break